/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Andre Meixner
* @copyright  2017 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#include <MMM/FactoryPluginLoader.h>
#include <MMM/Exceptions.h>
#include "MMMConverterConfiguration.h"
#include "MotionConverter.h"
#include "MotionConverterFactory.h"
#include <MMM/Motion/MotionWriterXML.h>
#include <MMM/Motion/MotionReaderXML.h>
#include <MMM/Model/ModelReaderXML.h>
#include <MMM/Model/ModelProcessorFactory.h>

MMM::ModelProcessorPtr setupModelProcessor(const std::string& modelProcessorName, const std::string& modelProcessorConfigFile)
{
    if (modelProcessorName.empty() || modelProcessorConfigFile.empty()) return nullptr;

    MMM_INFO << "Setting up model processor" << std::endl;

    MMM::ModelProcessorFactoryPtr modelFactory = MMM::ModelProcessorFactory::fromName(modelProcessorName, NULL);
    if (!modelFactory) throw MMM::Exception::MMMException("Could not create model processing factory of type '" + modelProcessorName + "'!");

    MMM::ModelProcessorPtr modelProcessor = modelFactory->createModelProcessor();
    if (!modelProcessor) throw MMM::Exception::MMMException("Could not create model processor of type '" + modelProcessorName + "'!");

    if (!modelProcessor->setupFile(modelProcessorConfigFile)) throw MMM::Exception::MMMException("Error while configuring model processor '" + modelProcessorName + "' from file '" + modelProcessorConfigFile + "'!");

    return modelProcessor;
}

int main(int argc, char *argv[])
{
    MMM_INFO << "--- MMM MOTION CONVERTER ---" << std::endl;

    MMMConverterConfiguration *configuration = new MMMConverterConfiguration();
    if (!configuration->processCommandLine(argc,argv)) {
        MMM_ERROR << "Could not process command line, aborting." << std::endl;
        return -1;
    }

    try {
        MMM_INFO << "Loading output model file" << std::endl;
        MMM::ModelReaderXMLPtr r(new MMM::ModelReaderXML());
        MMM::ModelPtr outputModel = r->loadModel(configuration->outputModelFilePath);
        if (!outputModel) throw MMM::Exception::MMMException("Could not read model from " + configuration->outputModelFilePath + "!");

        MMM::ModelProcessorPtr outputModelProcessor = setupModelProcessor(configuration->outputModelProcessor, configuration->outputModelConfigFilePath);
        MMM::ModelPtr outputModelProcessed = outputModel;
        if (outputModelProcessor) outputModelProcessed = outputModelProcessor->convertModel(outputModel);

        MMM::MotionReaderXMLPtr motionReader(new MMM::MotionReaderXML(configuration->sensorPluginPaths));
        MMM_INFO << "Reading motion file '" << configuration->inputMotionPath << "'!" << std::endl;
        MMM::MotionList motions = motionReader->loadAllMotions(configuration->inputMotionPath);
        MMM::MotionPtr motion = MMM::Motion::getMotion(motions, configuration->motionName);

        MMM_INFO << "--- Loading Converter Factory ---" << std::endl;
        boost::shared_ptr<MMM::FactoryPluginLoader<MMM::MotionConverterFactory> > factoryPluginLoader(new MMM::FactoryPluginLoader<MMM::MotionConverterFactory>(configuration->converterPluginPaths));
        MMM::MotionConverterFactoryPtr factory = factoryPluginLoader->loadFactory(configuration->converterName);
        if (!factory) return -3;

        MMM_INFO << "--- Creating Converter ---" << std::endl;
        MMM::MotionConverterPtr converter = factory->createMotionConverter(motion, outputModelProcessed, outputModel, outputModelProcessor, configuration->converterConfigFilePath);
        MMM::MotionPtr outputMotion = converter->convertMotion();

        MMM_INFO << "Writing motions to " << configuration->outputMotionPath << std::endl;
        MMM::MotionWriterXML::replaceAndWriteMotions(outputMotion, motions, configuration->outputMotionPath);

        MMM_INFO << "--- END ---" << std::endl;

        return 0;
    }
    catch (MMM::Exception::MMMException &e) {
        MMM_ERROR << e.what() << std::endl;
        return -2;
    }
}
